/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       can_receive.c/h
  * @brief      there is CAN interrupt function  to receive motor data,
  *             and CAN send function to send motor current to control motor.
  *             这里是CAN中断接收函数，接收电机数据,CAN发送函数发送电机电流控制电机.
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. support hal lib
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */

#include "CAN_receive.h"
#include "cmsis_os.h"
#include "OLED.h"
#include "bsp_rng.h"
#include "main.h"
#include "debug_config.h"
#include "detect_task.h"
#include "robot_config.h"

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
// motor data read
#define GET_MOTOR_MEASURE(ptr, data)                                    \
    {                                                                   \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6];                                   \
    }

#define GET_NUC_MEASURE(ptr,data)                                       \
    {                                       \
        (ptr)->x=(int8_t)(data)[0];   \
        (ptr)->y=(int8_t)(data)[1];   \
        (ptr)->w=(int8_t)(data)[2];   \
        (ptr)->pitch=(int8_t)(data)[3];   \
        (ptr)->yaw=(int16_t)((data)[5]<<8|(data)[4])>>2;   \
        (ptr)->yawWithoutIMU=(int8_t)(data)[6];   \
        (ptr)->pitchWithoutIMU=(int8_t)(data)[7];   \
        (ptr)->shoot=(uint8_t)((data)[4]&0x02)>>1;   \
        (ptr)->shootUp=(uint8_t)((data)[4]&0x01);   \
        gimbal_set_read_mark();                    \
        shoot_set_read_mark();                   \
        detect_hook(MINI_PC_TOE);                \
    }                                       
static motor_measure_t motor_robot[LENGTH_OF_MOTOR_ID];
static CAN_TxHeaderTypeDef can_tx_message;
static uint8_t can_send_data[8];
#ifdef SENTRY
betweenTwoSTM_t betweenTwoSTMMessage;
#endif
#ifdef USE_CAN
static fromNUC_t nuc_ctrl; 
static uint8_t gimbalreadmark = NUC_DATA_NOT_AVAILABLE;  //数据使用标志，值为0表明数据不可用（已使用过），值为1表明数据可用（被接收后尚未被使用）
static uint8_t shootreadmark = NUC_DATA_NOT_AVAILABLE;
#endif

#ifdef STM_STUDIO_CAN_SEND
#ifdef SENTRY_GIMBAL
int16_t shootDownFricLSend;
int16_t shootDownFricRSend;
int16_t shootDownTriggerSend;
#endif

#endif

#ifdef CAN_RECEIVE
#ifdef HERO
int16_t yawMotorReceive;
int16_t pitchMotorReceive;
int16_t shootMotorReceive;
int16_t triggerMotorReceive;
#endif
#ifdef SENTRY_GIMBAL
int16_t shootDownFricLReceive;
int16_t shootDownFricRReceive;
int16_t shootDownTriggerReceive;
#endif
#endif

#ifdef SENTRY_GIMBAL
static power_and_heat_t powerAndHeat;
#endif
/**
 * @brief          hal库CAN回调函数,接收电机数据
 * @param[in]      hcan:CAN句柄指针
 * @retval         none
 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) {
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
#ifdef SENTRY
#ifdef SENTRY_CHASSIS
    if (hcan == &CHASSIS_GIMBAL_CAN) {
        switch (rx_header.StdId) {
            case CAN_G_STM_CAN1_ID: {
                betweenTwoSTMMessage.x = (int8_t)rx_data[2];
                betweenTwoSTMMessage.y = (int8_t)rx_data[3];
                betweenTwoSTMMessage.w = (int8_t)rx_data[4];
                betweenTwoSTMMessage.nextmode = (int8_t)rx_data[5];
                betweenTwoSTMMessage.yawAngle=(uint16_t)(rx_data[6]<<8|rx_data[7]);
                detect_hook(GIMBALSTM32_TOE);
            } break;

            case CAN_YAW_DOWN: {
                GET_MOTOR_MEASURE(&motor_robot[YAW_DOWN], rx_data);
                detect_hook(YAW_DOWN + CHASSIS_LF_M_TOE);
            }

            case CAN_FROM_NUC:{
                GET_NUC_MEASURE(&nuc_ctrl,rx_data);
            }

            default: {
                break;
            }
        }
    } else if (hcan == &CHASSIS_CAN) {
        switch (rx_header.StdId) {
            case CAN_CHASSIS_LF_G:
            case CAN_CHASSIS_RF_G:
            case CAN_CHASSIS_RB_G:
            case CAN_CHASSIS_LB_G: {
                static uint8_t i = 0;
                // get motor id
                // 根据我们实际使用的修改
                i = rx_header.StdId - CAN_CHASSIS_LF_M + CHASSIS_LF_M;
                GET_MOTOR_MEASURE(&motor_robot[i], rx_data);
                detect_hook(i + CHASSIS_LF_M_TOE);
                break;
            }

            case CAN_CHASSIS_LF_M:
            case CAN_CHASSIS_RF_M:
            case CAN_CHASSIS_RB_M:
            case CAN_CHASSIS_LB_M: {
                static uint8_t i = 0;
                i = rx_header.StdId - CAN_CHASSIS_LF_M + CHASSIS_LF_M;
                GET_MOTOR_MEASURE(&motor_robot[i], rx_data);
                detect_hook(i + CHASSIS_LF_M_TOE);
                break;
            }

            default: {
                break;
            }
        }
    }
#endif
#ifdef SENTRY_GIMBAL
    if (hcan == &hcan1) {
        switch (rx_header.StdId) {
            #ifndef CAN_RECEIVE
            case CAN_SHOOT_UP_TRIGGER: {
                GET_MOTOR_MEASURE(&motor_robot[SHOOT_UP_TRIGGER], rx_data);
                detect_hook(SHOOT_UP_TRIGGER + YAW_UP_TOE);
                break;
            }
            case CAN_YAW_DOWN: {
                GET_MOTOR_MEASURE(&motor_robot[YAW_DOWN], rx_data);
                detect_hook(YAW_DOWN + YAW_UP_TOE);
                break;
            }
            case CAN_FROM_NUC:{
                GET_NUC_MEASURE(&nuc_ctrl,rx_data);
            }
            #else
            case CAN_M_DOWN_CAN2_ID:
            shootDownFricLReceive=(int16_t)(rx_data[4]<<8|rx_data[5]);
            shootDownFricRReceive=(int16_t)(rx_data[6]<<8|rx_data[7]);
            break;
            case CAN_M_UP_CAN2_ID:
            shootDownTriggerReceive=(int16_t)(rx_data[4]<<8|rx_data[5]);
            break;
            #endif
            case CAN_HEAT_AND_POWER:{
                powerAndHeat.heatLimte=(uint16_t)(rx_data[3]<<8|rx_data[4]);
                powerAndHeat.heat=(uint16_t)(rx_data[5]<<8|rx_data[6]);
            }
            default: {
                break;
            }
        }
    } else if (hcan == &hcan2) {
        switch (rx_header.StdId) {
            #ifndef CAN_RECEIVE
            case CAN_PITCH_UP: {
                GET_MOTOR_MEASURE(&motor_robot[PITCH_UP], rx_data);
                detect_hook(PITCH_UP + YAW_UP_TOE);
                break;
            }
            case CAN_SHOOT_UP_FRIC_L: {
                GET_MOTOR_MEASURE(&motor_robot[SHOOT_UP_FRIC_L], rx_data);
                detect_hook(SHOOT_UP_FRIC_L + YAW_UP_TOE);
                break;
            }
            case CAN_SHOOT_UP_FRIC_R: {
                GET_MOTOR_MEASURE(&motor_robot[SHOOT_UP_FRIC_R], rx_data);
                detect_hook(SHOOT_UP_FRIC_R + YAW_UP_TOE);
                break;
            }
            case CAN_YAW_UP: {
                GET_MOTOR_MEASURE(&motor_robot[YAW_UP], rx_data);
                detect_hook(YAW_UP + YAW_UP_TOE);
                break;
            }
            case CAN_PITCH_DOWN: {
                GET_MOTOR_MEASURE(&motor_robot[PITCH_DOWN], rx_data);
                detect_hook(PITCH_DOWN + YAW_UP_TOE);
                break;
            }
            case CAN_SHOOT_DOWN_FRIC_L: {
                GET_MOTOR_MEASURE(&motor_robot[SHOOT_DOWN_FRIC_L], rx_data);
                detect_hook(SHOOT_DOWN_FRIC_L + YAW_UP_TOE);
                break;
            }
            case CAN_SHOOT_DOWN_FRIC_R: {
                GET_MOTOR_MEASURE(&motor_robot[SHOOT_DOWN_FRIC_R], rx_data);
                detect_hook(SHOOT_DOWN_FRIC_R + YAW_UP_TOE);
                break;
            }
            case CAN_SHOOT_DOWN_TRIGGER: {
                GET_MOTOR_MEASURE(&motor_robot[SHOOT_DOWN_TRIGGER], rx_data);
                detect_hook(SHOOT_DOWN_TRIGGER + YAW_UP_TOE);
                break;
            }
            #else
            case CAN_M_DOWN_CAN2_ID:
            shootDownFricLReceive=(int16_t)(rx_data[4]<<8|rx_data[5]);
            shootDownFricRReceive=(int16_t)(rx_data[6]<<8|rx_data[7]);
            break;
            case CAN_M_UP_CAN2_ID:
            shootDownTriggerReceive=(int16_t)(rx_data[4]<<8|rx_data[5]);
            break;
            #endif
            default: {
                break;
            }
        }
    }
#endif
#else
    #ifndef CAN_RECEIVE
    switch (rx_header.StdId) {
        case CAN_3508_M1_ID:
        case CAN_3508_M2_ID:
        case CAN_3508_M3_ID:
        case CAN_3508_M4_ID:
        case CAN_YAW_MOTOR_ID:
        case CAN_PIT_MOTOR_ID:
        case CAN_TRIGGER_MOTOR_ID: {
            static uint8_t i = 0;
            // get motor id
            // 根据我们实际使用的修改
            i = rx_header.StdId - CAN_3508_M1_ID;
            if ((hcan == &SHOOT_CAN) && (i == 0 || i == 1)) {
                GET_MOTOR_MEASURE(&motor_robot[i+SHOOT_FRIC_L], rx_data);
                detect_hook(SHOOT_FRIC_L_TOE + i);
                break;
            }

            GET_MOTOR_MEASURE(&motor_robot[i], rx_data);
            detect_hook(CHASSIS_LF_M_TOE + i);
            break;
        }

        case CAN_FROM_NUC:{
            GET_NUC_MEASURE(&nuc_ctrl,rx_data);
        }

        default: {
            break;
        }
    }
    #else
    if(hcan == &SHOOT_CAN){
        switch (rx_header.StdId){
            case CAN_GIMBAL_ALL_ID:
            pitchMotorReceive=(int16_t)(rx_data[2]<<8|rx_data[3]);
            triggerMotorReceive=(int16_t)(rx_data[4]<<8|rx_data[5]);
            break;
            case CAN_SHOOT_ALL_ID:
            shootMotorReceive=(int16_t)(rx_data[0]<<8|rx_data[1]);
            break;
            default: {
                break;
            }
        }
    }else{
        switch (rx_header.StdId){
            case CAN_YAW_CAN1_ID:
            yawMotorReceive=(int16_t)(rx_data[0]<<8|rx_data[1]);
            break;
            default: {
                break;
            }
        }
    }

    #endif

#endif
}

#ifdef SENTRY
#ifdef SENTRY_CHASSIS
void CAN_cmd_chassis_M3508(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_CHASSIS_M_CAN2_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = motor1 >> 8;
    can_send_data[1] = motor1;
    can_send_data[2] = motor2 >> 8;
    can_send_data[3] = motor2;
    can_send_data[4] = motor3 >> 8;
    can_send_data[5] = motor3;
    can_send_data[6] = motor4 >> 8;
    can_send_data[7] = motor4;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &can_tx_message, can_send_data, &send_mail_box);
}

void CAN_cmd_chassis_GM6020(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_CHASSIS_G_CAN1_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = motor1 >> 8;
    can_send_data[1] = motor1;
    can_send_data[2] = motor2 >> 8;
    can_send_data[3] = motor2;
    can_send_data[4] = motor3 >> 8;
    can_send_data[5] = motor3;
    can_send_data[6] = motor4 >> 8;
    can_send_data[7] = motor4;
    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &can_tx_message, can_send_data, &send_mail_box);
}
#endif
#ifdef SENTRY_GIMBAL
void CAN_cmd_shoot_up_stm(int16_t shootUp)//shootdown
{
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_M_CAN1_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = 0;
    can_send_data[1] = 0;
    can_send_data[2] = 0;
    can_send_data[3] = 0;
    can_send_data[4] = shootUp >> 8;
    can_send_data[5] = shootUp;
    can_send_data[6] = 0;
    can_send_data[7] = 0;
    
    HAL_CAN_AddTxMessage(&CHASSIS_GIMBAL_CAN, &can_tx_message, can_send_data, &send_mail_box);
}

void CAN_cmd_yaw_down(int16_t yawDown)//shootdown
{
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_G_STM_CAN1_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = yawDown >> 8;
    can_send_data[1] = yawDown;
    can_send_data[2] = betweenTwoSTMMessage.x;
    can_send_data[3] = betweenTwoSTMMessage.y;
    can_send_data[4] = betweenTwoSTMMessage.w;
    can_send_data[5] = betweenTwoSTMMessage.nextmode;
    can_send_data[6] = betweenTwoSTMMessage.yawAngle>>8;
    can_send_data[7] = betweenTwoSTMMessage.yawAngle;
    HAL_CAN_AddTxMessage(&CHASSIS_GIMBAL_CAN, &can_tx_message, can_send_data, &send_mail_box);
}

void CAN_cmd_GIMBAL_CAN_G(int16_t yawUp, int16_t pitchDown, int16_t pitchUp)//gimbaldown
{
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_G_CAN2_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = yawUp >> 8;
    can_send_data[1] = yawUp;
    can_send_data[2] = pitchDown >> 8;
    can_send_data[3] = pitchDown;
    can_send_data[4] = pitchUp >> 8;
    can_send_data[5] = pitchUp;
    can_send_data[6] = 0;
    can_send_data[7] = 0;
    HAL_CAN_AddTxMessage(&GIMBAL_CAN, &can_tx_message, can_send_data, &send_mail_box);
}

void CAN_cmd_GIMBAL_CAN_M_up(int16_t shootUpFricL, int16_t shootUpFricR, int16_t triggerDown)//shootdown
{
    #ifdef LED_CAN_SEND
    aRGB_led_show(0xFF00FF00);
    #endif
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_M_UP_CAN2_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = 0;
    can_send_data[1] = 0;
    can_send_data[2] = shootUpFricL >> 8;
    can_send_data[3] = shootUpFricL;
    can_send_data[4] = shootUpFricR >> 8;
    can_send_data[5] = shootUpFricR;
    can_send_data[6] = triggerDown >> 8;
    can_send_data[7] = triggerDown;
    #ifdef STM_STUDIO_CAN_SEND
    shootDownTriggerSend = triggerDown;
    #endif
    HAL_CAN_AddTxMessage(&GIMBAL_CAN, &can_tx_message, can_send_data, &send_mail_box);
}

void CAN_cmd_GIMBAL_CAN_M_down(int16_t shootDownFricL, int16_t shootDownFricR)//gimbaldown
{
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_M_DOWN_CAN2_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = 0;
    can_send_data[1] = 0;
    can_send_data[2] = 0;
    can_send_data[3] = 0;
    can_send_data[4] = shootDownFricL >> 8;
    can_send_data[5] = shootDownFricL;
    can_send_data[6] = shootDownFricR >> 8;
    can_send_data[7] = shootDownFricR;
    #ifdef STM_STUDIO_CAN_SEND
    shootDownFricLSend = shootDownFricL ;
    shootDownFricRSend = shootDownFricR ;
    #endif
    HAL_CAN_AddTxMessage(&GIMBAL_CAN, &can_tx_message, can_send_data, &send_mail_box);
}
#endif
#else
void CAN_cmd_gimbal(int16_t pitch, int16_t shoot) {
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_GIMBAL_ALL_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = 0;
    can_send_data[1] = 0;
    can_send_data[2] = (pitch >> 8);
    can_send_data[3] = pitch;
    can_send_data[4] = (shoot >> 8);
    can_send_data[5] = shoot;
    can_send_data[6] = 0;
    can_send_data[7] = 0;
    HAL_CAN_AddTxMessage(&GIMBAL_CAN, &can_tx_message, can_send_data, &send_mail_box);
}

void CAN_cmd_yaw(int16_t yaw) {
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_YAW_CAN1_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = (yaw >> 8);
    can_send_data[1] = yaw;
    can_send_data[2] = 0;
    can_send_data[3] = 0;
    can_send_data[4] = 0;
    can_send_data[5] = 0;
    can_send_data[6] = 0;
    can_send_data[7] = 0;
    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &can_tx_message, can_send_data, &send_mail_box);
}

void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4) {
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_CHASSIS_ALL_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = motor1 >> 8;
    can_send_data[1] = motor1;
    can_send_data[2] = motor2 >> 8;
    can_send_data[3] = motor2;
    can_send_data[4] = motor3 >> 8;
    can_send_data[5] = motor3;
    can_send_data[6] = motor4 >> 8;
    can_send_data[7] = motor4;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &can_tx_message, can_send_data, &send_mail_box);
}

void CAN_cmd_shoot(int16_t motor5, int16_t motor6) {
    uint32_t send_mail_box;
    can_tx_message.StdId = CAN_SHOOT_ALL_ID;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    can_send_data[0] = (motor5 >> 8);
    can_send_data[1] = motor5;
    can_send_data[2] = (motor6 >> 8);
    can_send_data[3] = motor6;
    can_send_data[4] = 0;
    can_send_data[5] = 0;
    can_send_data[6] = 0;
    can_send_data[7] = 0;
    HAL_CAN_AddTxMessage(&SHOOT_CAN, &can_tx_message, can_send_data, &send_mail_box);
}
#endif

const motor_measure_t *get_motor_measure_point(uint8_t i)
{
    return &motor_robot[i];
}

#ifdef SENTRY
betweenTwoSTM_t* get_between_two_STM_message_point(void) {
    return &betweenTwoSTMMessage;
}
#endif

#ifdef USE_CAN
const fromNUC_t *get_nuc_control_point(void)
{
    return (const fromNUC_t*)&nuc_ctrl;
}

//标记数据的接收状态
//云台
void gimbal_set_read_mark(void)
{
    gimbalreadmark = NUC_DATA_AVAILABLE;   //数据刚刚被接收，尚未使用
}

void gimbal_clear_read_mark(void)
{
    gimbalreadmark = NUC_DATA_NOT_AVAILABLE;
}

int gimbal_NUC_is_available(void)
{
    return (gimbalreadmark == NUC_DATA_AVAILABLE);
}
//发射
void shoot_set_read_mark(void)
{
    shootreadmark = NUC_DATA_AVAILABLE;   //数据刚刚被接收，尚未使用
}

void shoot_clear_read_mark(void)
{
    shootreadmark = NUC_DATA_NOT_AVAILABLE;
}

int shoot_NUC_is_available(void)
{
    return (shootreadmark == NUC_DATA_AVAILABLE);
}

int found_armor(void)
{
    if(toe_is_error(MINI_PC_TOE))
        return 0;
    if(nuc_ctrl.shoot == 1)
        return 1;
    else
        return 0;
}

int found_armorUp(void)
{
    if(toe_is_error(MINI_PC_TOE))
        return 0;
    if(nuc_ctrl.shootUp == 1)
        return 1;
    else
        return 0;
}
#ifdef SENTRY_GIMBAL
uint8_t get_shoot_heat_limit(void){
    return powerAndHeat.heatLimte;
}
uint8_t get_shoot_heat_1(void){
    return powerAndHeat.heat;
}
#endif


/*********************************发送任务***************************************************/

#define ROBOT_NUC_TASK_INIT_TIME 1000  // 1000ms初始化
#define ROBOT_NUC_TASK_TIME 1000  // 1000ms执行一次

#define CAN_TO_NUC 0x006

static uint8_t sendToNuc[8];

void CAN_cmd_nuc(void) {
    uint32_t send_mail_box;   
    can_tx_message.StdId = CAN_TO_NUC;
    can_tx_message.IDE = CAN_ID_STD;
    can_tx_message.RTR = CAN_RTR_DATA;
    can_tx_message.DLC = 0x08;
    sendToNuc[3]=get_shoot_heat_limit()>>8;
    sendToNuc[4]=get_shoot_heat_limit();
    sendToNuc[5]=get_shoot_heat_1()>>8;
    sendToNuc[6]=get_shoot_heat_1();
    HAL_CAN_AddTxMessage(&CHASSIS_GIMBAL_CAN, &can_tx_message, sendToNuc, &send_mail_box);
}
#ifdef SENTRY_CHASSIS
void to_nuc_send_task(void const *pvParameters)
{
    osDelay(ROBOT_NUC_TASK_INIT_TIME);
    while(1)
    {
        
        CAN_cmd_nuc();
        osDelay(ROBOT_NUC_TASK_TIME);
    }   
}
#endif
#endif

#ifdef OLED_CAN_RECEIVE
void oled_can_receive(void){
	OLED_printf(0,0,"x:%d y:%d w:%d yaw:%d pitch:%d yawUp:%d pitchUp:%d d_shoot:%d u_shoot:%d",
					nuc_ctrl.x,nuc_ctrl.y,nuc_ctrl.w,
					nuc_ctrl.yaw,nuc_ctrl.pitch,
					nuc_ctrl.yawWithoutIMU,nuc_ctrl.pitchWithoutIMU,
					nuc_ctrl.shoot,nuc_ctrl.shootUp);
}
#endif
